
#include <set>


#include "common.h"
#include "intersect_3d.h"
#include "intersect_2d.h"
#include "meshintersection.h"
#include "booleanmesh.h"


MeshType mesh1 ;
MeshType mesh2 ;
MeshType mesh3 ;

//std::vector<std::deque<Vector <double> > > v_curve; 

	 


struct timeval tmstart, tmend;    
struct timeval genstart, genend;  
struct timezone tzp; 


/*
 * printElapsed (calculate & print out time elapsed)
 */
void printElapsed (const char *desc, struct timeval *start, struct timeval *end, int niters) 
{
        struct timeval elapsed;
        if (start -> tv_usec > end -> tv_usec) 
        {
                end -> tv_usec += 1000000;
                end -> tv_sec--;
        }

        elapsed.tv_usec = end -> tv_usec - start -> tv_usec;
        elapsed.tv_sec = end -> tv_sec - start -> tv_sec;

        printf("\n %s total elapsed time = %1d (usec) \n",
			   desc, int((elapsed.tv_sec*1000000 + elapsed.tv_usec)/niters));

}/* printElapsed() */



#define FAST // Use AABBTree switch. 

#ifdef FAST

bool is_vertices_on_hole (VertexType * v)
{
	HalfEdgeType * he = (HalfEdgeType *) v->he; 
	HalfEdgeType * end = he; 
	do 
	{
		if (he->flip == NULL) return true; 
		he = (HalfEdgeType * )he->flip->next ;
	} while (end != he);
	return false; 
}

HalfEdgeType * find_boundary_he (VertexType * v)
{
	HalfEdgeType * he = (HalfEdgeType *) v->he; 
	HalfEdgeType * end = he; 
	do 
	{
		if (he->flip == NULL) return he; 
		he = (HalfEdgeType * )he->flip->next ;
	} while (end != he);
	throw ("Not a boundary vertex");
	return NULL ;
}
void fill_one_hole (VertexType * v, MeshType & mesh)
{
	// traverse the hole , haha
	HalfEdgeType * he = find_boundary_he (v ); 
	HalfEdgeType * end  = he; 
	std::vector<unsigned > v_idx ;
	do 
	{
		std::cout<<he->from_v->m_idx<<' ';
		v_idx.push_back (he->from_v->m_idx);
		he = find_boundary_he( (VertexType *)he->next->from_v ) ;
	} while (he != end);
	std::cout<<std::endl;
	reverse(v_idx.begin(), v_idx.end());
	mesh.construct_face2(mesh, v_idx) ;

}
void fill_hole (MeshType & mesh)
{
	// Naive sequential sequential search. 
	MeshType::VertexIterator  v_iter; 

	for (v_iter = mesh.vertices.begin(); v_iter != mesh.vertices.end(); ++v_iter)
	{
		if (is_vertices_on_hole(v_iter->second))
		{
			std::cout<<v_iter->second->m_idx<<' '<<v_iter->second->m_coord<<std::endl;
			fill_one_hole(v_iter->second, mesh);
		}
	}
}

void find_ceiling (MeshType & mesh, std::vector <HalfEdgeType *> & v_he, std::vector<VertexType *> & v_vertex)
{
	MeshType::HalfEdgeIterator heIter; 
	for (heIter = mesh.hash_he.begin(); heIter != mesh.hash_he.end(); ++heIter)
	{
		
		if (heIter->second->flip == NULL) 
		{
			HalfEdgeType * he = (HalfEdgeType * )heIter->second ; 
			std::cout<<'('<<he->from_v->m_idx <<',' << he->next->from_v->m_idx<<')'; 
			v_he.push_back (he) ;
			v_vertex.push_back ((VertexType *) he->from_v);
		}
	}
		std::cout<<std::endl;
}
void build_correspond_vertex (std::vector<VertexType *> & vv1, std::vector<VertexType *> & vv2, std::map<unsigned, unsigned> & vv_relation)
{
	for (unsigned i = 0; i < vv1.size(); ++i) std::cout<<vv1[i]->m_idx<<' ';
	std::cout<<std::endl;
	for (unsigned i = 0; i< vv1.size(); ++i)
	{
		for (unsigned j = 0; j < vv2.size(); ++j)
		{
			if ((vv1[i]->m_coord - vv2[j]->m_coord ).square_module() < tiny_value)
			{
				std::cout<<vv1[i]->m_idx<<'-'<<vv2[j]->m_idx<<std::endl;
				vv_relation.insert (std::make_pair (vv1[i]->m_idx, vv2[j]->m_idx));
			}
		}
	}
}
unsigned translate (std::map<unsigned, unsigned > & vv_relation, unsigned idx)
{
	/*
	for (unsigned i = 0; i < vv_relation.size(); ++i)
	{
		if (vv_relation[i].first == idx) return vv_relation[i].second; 
	}
	throw ("Fail to translate");
	*/
	std::map<unsigned, unsigned>::iterator iter = vv_relation.find(idx);
	if (iter == vv_relation.end()) throw  ("Fail to translate");
	return iter->second; 
}
void build_correspond_he (std::vector <HalfEdgeType *> & v_he1, std::vector<HalfEdgeType * > & v_he2, std::map<unsigned, unsigned > & vv_relation)
{
	for (unsigned i = 0; i < v_he1.size(); ++ i)
	{
		unsigned id1 = v_he1[i]->from_v->m_idx ;
		unsigned id2 = v_he1[i]->next->from_v->m_idx ;
		std::cout<<"Translating "<<id1<<' '<<id2<<std::endl;
		id1 = translate (vv_relation, id1);
		id2 = translate (vv_relation, id2);
		for (unsigned j = 0; j < v_he2.size(); ++j)
		{
			unsigned id3 = v_he2[j]->from_v->m_idx ;
			unsigned id4 = v_he2[j]->next->from_v->m_idx ;
			if (id1 == id4 && id2 == id3)
			{
				//std::cout<<"Mach"<<std::endl;
				v_he1[i]->flip = v_he2[j];
				v_he2[j]->flip = v_he1[i];
				break; 
			}
		}
		
	}
}
void mesh_intersection (MeshType & mesh1, MeshType & mesh2)
{
	//mesh1.mesh_intersection(mesh2);
	mesh1.mesh_intersection_relation(mesh2);
	mesh1.p_mesh2 = & mesh2 ;
	mesh2.p_mesh2 = & mesh1 ;
	// Set which part is taken. 
	mesh1.pt_type = MeshType::OUT |  MeshType::ON;
	mesh2.pt_type = MeshType::OUT ;

	
	// Split mesh1. 
	std::cout<<"Split Mesh1"<<std::endl;
	mesh1.split_mesh (mesh1, mesh2); 

	// Split mesh2. 
	std::cout<<"Split Mesh2"<<std::endl;
	mesh2.split_mesh (mesh2, mesh1);


	std::vector<HalfEdgeType * > v_he1; 
	std::vector<VertexType * > v_vertex1;
	find_ceiling (mesh1, v_he1, v_vertex1);
	

	std::vector<HalfEdgeType * > v_he2 ;
	std::vector<VertexType * > v_vertex2;
	find_ceiling (mesh2, v_he2, v_vertex2);

	std::map<unsigned, unsigned > vv_relation;
	build_correspond_vertex (v_vertex1, v_vertex2, vv_relation );

//	for (unsigned i = 0; i < vv_relation.size(); ++i)
//	{
//		std::cout<<vv_relation[i].first<<'-'<<vv_relation[i].second<<std::endl;
//	}

	build_correspond_he(v_he1, v_he2, vv_relation);

	// Construct the result mesh. 
	
	FaceType * begin_face = mesh1.faces[mesh1.v_new_face_id[0]]; 
	mesh1.copy_mesh (mesh1, mesh2, begin_face, mesh3, vv_relation);


}

#else


void mesh_intersection (MeshType & mesh1, MeshType & mesh2)
{
	for (MeshType::FaceIterator iter1 = mesh1.faces.begin(); iter1 != mesh1.faces.end(); ++iter1)
	{
		for (MeshType::FaceIterator iter2 = mesh2.faces.begin(); iter2 != mesh2.faces.end(); ++iter2)
		{

			const PolygonType & p1 = (*iter1)->get_polygon();
			const PolygonType & p2 = (*iter2)->get_polygon();

			bool flag1 = predicate_face_face(*iter1, *iter2); 

			BooleanMeshIntersectResult<Value> intrRes; 
			bool flag2 = intersect_face_face (*iter1, *iter2, intrRes); 

			
			if (flag1 != flag2)
			{
				std::cout<< flag1<<' '<<(*iter1)->m_idx << ':' << p1 <<std::endl;;
				std::cout<< flag2<<' '<<(*iter2)->m_idx << ':' << p2 <<std::endl;;
			}
					  
			if (flag2)
			{
				if (intrRes.res_type == SEGMENT)
				{
					v_segment.push_back(intrRes.segment);
				}
				else if (intrRes.res_type == POLYGON)
				{
					v_polygon.push_back(intrRes.polygon);
				}
				v_face_id.insert((*iter2)->m_idx) ;

				//std::cout<<"Intersect"<<std::endl;
				//std::cout<<p1<<std::endl;
				//std::cout<<p2<<std::endl;
			}
		}
	}
}

/*
// Tow hierarchy intersection. 
void mesh_intersection (MeshType & mesh1, MeshType & mesh2)
{
	AABBTreePolyType * tree1 , * tree2;
	AABBTreePolyBuilderType builder1, builder2 ; 

	AABBTreePolyType::AABBTreeColResult aabbtreeColResult; 


	LATENCY(tree1 = builder1.build_from_mesh (mesh1 ,1), "Build AABBTree1. ");
	LATENCY(tree2 = builder2.build_from_mesh (mesh2 ,1), "Build AABBTree2. ");

	tree1->collideWithAABBTree ( * tree2, aabbtreeColResult);

	std::cout<<aabbtreeColResult.poly_pairs.size()<<std::endl;

	for (unsigned i = 0; i < aabbtreeColResult.poly_pairs.size(); ++i)
	{
		std::vector<VectorType * > v1 ;
		mesh1.faces[aabbtreeColResult.poly_pairs[i].first]->Face2VertexCoord(v1);
		PolygonType p1(v1); 
        //v_face_id.insert(aabbtreeColResult.poly_pairs[i].first);
				

		std::vector<VectorType * > v2 ;
		mesh2.faces[aabbtreeColResult.poly_pairs[i].second]->Face2VertexCoord(v2);
		PolygonType p2(v2); 
		v_face_id.insert(aabbtreeColResult.poly_pairs[i].second);

				
		IntersectResult<Value> intrRes; 
		if (intersect_tri_tri (p1,p2, intrRes))
		{
			if (intrRes.res_type == SEGMENT)
			{
				v_segment.push_back(intrRes.segment);
			}
			else if (intrRes.res_type == POLYGON)
			{
				v_polygon.push_back(intrRes.polygon);
			}
			//v_face_id.insert((*iter2)->m_idx) ;
			//std::cout<<(*iter1)->m_idx <<' '<<(*iter2)->m_idx<<std::endl;
			//std::cout<<"Intersect"<<std::endl;
			//std::cout<<p1<<std::endl;
			//std::cout<<p2<<std::endl;
		}
				
	}
	delete tree1 ;  
	delete tree2 ;
}
*/

#endif




